/*
 * @Author: Colin Dai
 * @Date: 2022-04-29 21:13:56
 * @LastEditors: Colin Dai
 * @LastEditTime: 2022-05-18 21:25:49
 * @FilePath: \intelligent-car\src\main.ino
 * @Description: 
 * dailianzheng@163.com
 * Copyright (c) 2022 by Colin.Dai, All Rights Reserved. 
 */


#include "wheel_ctrl.h"
#include <SoftwareSerial.h>




//按键值定义
#define KEY_NULL 0

#define KEY_UP 64511
#define KEY_DOWN 61439
#define KEY_LEFT 63487
#define KEY_RIGHT 57343

#define KEY_SANJIAO 65527
#define KEY_CHA 65533
#define KEY_FANGKUANG 65534
#define KEY_YUAN 65531

#define KEY_STAR_SELECT 16383
#define KEY_SELECT 49151
#define KEY_START 32767

#define KEY_L1 65023
#define KEY_R1 65519
#define KEY_L2 65279
#define KEY_R2 65503


/**
 * @brief 全局变量定义
 * 
 */


#define DEFAULT_SPEED 80
#define SPEED_STEP 40
#define SPEED_MAX 240

#define TURN_DELAY 200

//初始速度
int speed  = DEFAULT_SPEED;

int MODE_TRUN = 0; //如果为1，一直旋转

SoftwareSerial MySerial(D5, D6);//RX,TX

boolean g_isRunning = false;

//接收数据相关
const byte numBytes = 8;//接收8位数据（发送端是8个数据需要去头尾校验数据）
byte Read_Bytes[numBytes];//无线模块串口数据存储变量
byte numReceived = 0;//接收数据计数标识方便面查看读取到十六进制数量
boolean newData = false;//初始接收标识



/**
 * @brief 初始化
 * @param {*}
 * @return {*}
 */
void setup() 
{
  
  //系统打印串口
  Serial.begin(9600);
  MySerial.begin(9600);

  //初始化底盘控制
  init_wheel();

  adjustSpeed(speed);  

  //延时等待系统初始化
  delay(500); 

  Serial.println("Init ok!");
}


/**
 * @brief 主循环
 * @param {*}
 * @return {*}
 */
void loop() 
{  

  int key = KEY_NULL;

  //检测无线指令
  key = get_key(); 

  Serial.println(key);

  switch (key)
  {
  case KEY_UP: //前进
    car_forward();
    Serial.println("car_forward");
    g_isRunning = true;
    break;
  case KEY_DOWN: //后退
    car_backward();
    Serial.println("car_backward");
    break;
  case KEY_YUAN: //停止
    car_stop();
    speed = DEFAULT_SPEED;
    adjustSpeed(speed);  
    Serial.println("stop");
    g_isRunning = false;
    break;
  case KEY_FANGKUANG: //模式切换
    MODE_TRUN = !MODE_TRUN;  
    Serial.println("mode change");
    break;  
  case KEY_LEFT: //左转
    if(MODE_TRUN)
    {
      car_stop();
      car_left();
    }
    else
    {
      car_stop();
      car_left();
      delay(TURN_DELAY);
      car_stop();
      if(g_isRunning) car_forward();     
    }
    Serial.println("turn left");
    
    break;
  case KEY_RIGHT: //后转
    if(MODE_TRUN)
    {
      car_stop();
      car_right();
    }
    else
    {
      car_stop();
      car_right();
      delay(TURN_DELAY);
      car_stop();
      if(g_isRunning) car_forward();     
    }
    Serial.println("turn right");
    break;
  case KEY_SANJIAO: //加速
    if (speed < SPEED_MAX)
    {
      Serial.println("speed ++");
      speed += SPEED_STEP;
      adjustSpeed(speed);  
      Serial.println(speed);
    }    
    break;
  case KEY_CHA: //降速
    if (speed > DEFAULT_SPEED)
    {
      Serial.println("speed --");
      speed -= SPEED_STEP;
      adjustSpeed(speed);  
    }  
break;
  
  default:
    break;
  }

  delay(500);//不要检测太快  
}

//====================处理获取按键的值=========================
int get_key() {//串口获取到的数据进行原始数据恢复处理
	int key = KEY_NULL;
	
	//读取数据
	RX_joystick_data();

    if (newData == true) {//判断如果数组接收完毕了，就可以开始处理实现自己的功能了
      key = word(Read_Bytes[0],Read_Bytes[1]);//将读取到的高8位和低位的按键值转换成16位的按键值
      int LX=Read_Bytes[2];//将读取到的左摇杆的垂直值送给变量
      int LY=Read_Bytes[3];//将读取到的左摇杆的水平值送给变量
      int RX=Read_Bytes[4];//将读取到的右摇杆的垂直值送给变量
      int RY=Read_Bytes[5];//将读取到的右摇杆的水平值送给变量      
      newData = false;//数据接收完毕标识关闭
    }
	
	return key;
}

//====================接收数据函数=========================
void RX_joystick_data() {//十六进制接收处理里函数
    static boolean ZZS = false;//正在接收数据的标识
    static byte ndx = 0;//用于接收数据计数变量
    byte start_MA = 0xAA;//启动标记
    byte end_MA = 0xBB;//结束标记
    byte rb;//串口数据接收缓存变量
    while (MySerial.available() > 0 && newData == false) {//当开始接收到数据
        rb = MySerial.read();//接收串口数据
        if (ZZS == true) {//判断“正在接收数据的标识”
            if (rb != end_MA) {//判断是否接收到了结束标记
                Read_Bytes[ndx] = rb;//不是结束标识的话，将接收的数据写入接收数组中
                ndx++;//接收数据增加1位
                if (ndx >= numBytes) {//判断是否接收够了设置的数组数量
                    ndx = numBytes - 1;//就收够了就减去多余的数量
                }
            }
            else {//否则已经收到了结束标识               
                ZZS = false;//“正在接收数据的标识“设置为初始接收状态
                numReceived = ndx;  // 将接收到的数据数量送给另一个变量，方便调试时使用
                ndx = 0;//清零数组计数值
                newData = true;//整组数据接收完毕标识位
            }
        }
        else if (rb == start_MA) {//串口接收到了启动标识十六进制数值
            ZZS = true;//设置已经开始可以接收以下的数组了
        }
    }
}



